import numpy as np
import matplotlib.pyplot as plt
import cv2 as cv
import scipy.signal as ss


# 生成高斯核
def gauss_seperate_kernel(kernel_size, sigma=1):
    if sigma == 0:
        sigma = ((kernel_size - 1) * 0.5 - 1) * 0.3 + 0.8
    m = kernel_size[0]
    n = kernel_size[1]
    # 高斯核的可分解性
    x_m = np.arange(m) - m // 2  # //在Python中表示整数除法，返回不大于结果的一个最大的整数，意思就是除法结果向下取整
    y_m = (
        1
        / (np.sqrt(2 * np.pi) * sigma)
        * np.exp(-(x_m**2) / (2 * sigma**2)).reshape(-1, 1)
    )

    x_n = np.arange(n) - n // 2
    y_n = (
        1
        / (np.sqrt(2 * np.pi) * sigma)
        * np.exp(-(x_n**2) / (2 * sigma**2)).reshape(-1, 1)
    )
    kernel = y_m * y_n.T  # 没归一化
    return kernel


# 生成二维高斯x方向一阶导，即gx模板
def gauss_gx(kernel_size, sigma=1):
    if sigma == 0:
        sigma = ((kernel_size - 1) * 0.5 - 1) * 0.3 + 0.8
    m = kernel_size[0]
    n = kernel_size[1]
    kernel = np.zeros((m, n))
    x_m = np.arange(m - 1, -1, -1) - m // 2  # [2 1  0  -1  -2]
    y_n = np.arange(n - 1, -1, -1) - n // 2
    for i in x_m:
        for j in y_n:
            ki = m // 2 - i
            kj = n // 2 - j
            value = (
                -i
                * np.exp(-(i * i + j * j) / (2 * sigma * sigma))
                / (2 * np.pi * np.power(sigma, 4))
            )
            kernel[kj, ki] = value
    return kernel


# 生成二维高斯y方向一阶导，即gy模板
def gauss_gy(kernel_size, sigma=1):
    if sigma == 0:
        sigma = ((kernel_size - 1) * 0.5 - 1) * 0.3 + 0.8
    m = kernel_size[0]
    n = kernel_size[1]
    kernel = np.zeros((m, n))
    x_m = np.arange(m - 1, -1, -1) - m // 2  # [-2 -1  0  1  2]
    y_n = np.arange(n - 1, -1, -1) - n // 2
    for i in x_m:
        for j in y_n:
            ki = m // 2 - i
            kj = n // 2 - j
            value = (
                -j
                * np.exp(-(i * i + j * j) / (2 * sigma * sigma))
                / (2 * np.pi * np.power(sigma, 4))
            )
            kernel[kj, ki] = value
    return kernel


# 生成二维高斯x方向二阶导，即gxx模板
def gauss_gxx(kernel_size, sigma=1):
    if sigma == 0:
        sigma = ((kernel_size - 1) * 0.5 - 1) * 0.3 + 0.8
    m = kernel_size[0]
    n = kernel_size[1]
    kernel = np.zeros((m, n))
    x_m = np.arange(m - 1, -1, -1) - m // 2  # [-2 -1  0  1  2]
    y_n = np.arange(n - 1, -1, -1) - n // 2
    for i in x_m:
        for j in y_n:
            ki = m // 2 - i
            kj = n // 2 - j
            value = (
                (i**2 - sigma**2)
                * np.exp(-(i * i + j * j) / (2 * sigma * sigma))
                / (2 * np.pi * np.power(sigma, 6))
            )
            kernel[kj, ki] = value
    return kernel


# 生成二维高斯y方向二阶导，即gyy模板
def gauss_gyy(kernel_size, sigma=1):
    if sigma == 0:
        sigma = ((kernel_size - 1) * 0.5 - 1) * 0.3 + 0.8
    m = kernel_size[0]
    n = kernel_size[1]
    kernel = np.zeros((m, n))
    x_m = np.arange(m - 1, -1, -1) - m // 2  # [-2 -1  0  1  2]
    y_n = np.arange(n - 1, -1, -1) - n // 2
    for i in x_m:
        for j in y_n:
            ki = m // 2 - i
            kj = n // 2 - j
            value = (
                (j**2 - sigma**2)
                * np.exp(-(i * i + j * j) / (2 * sigma * sigma))
                / (2 * np.pi * np.power(sigma, 6))
            )
            kernel[kj, ki] = value
    return kernel


# 生成二维高斯xy方向二阶导，即gxy模板
def gauss_gxy(kernel_size, sigma=1):
    if sigma == 0:
        sigma = ((kernel_size - 1) * 0.5 - 1) * 0.3 + 0.8
    m = kernel_size[0]
    n = kernel_size[1]
    kernel = np.zeros((m, n))
    x_m = np.arange(m - 1, -1, -1) - m // 2  # [-2 -1  0  1  2]
    y_n = np.arange(n - 1, -1, -1) - n // 2
    for i in x_m:
        for j in y_n:
            ki = m // 2 - i
            kj = n // 2 - j
            value = (
                i
                * j
                * np.exp(-(i * i + j * j) / (2 * sigma * sigma))
                / (2 * np.pi * np.power(sigma, 6))
            )
            kernel[kj, ki] = value
    return kernel


def zero_pad(image, pad):
    """
    对图像进行零填充
    :param image: 原始图像，numpy数组，shape为(height, width)
    :param pad: 填充的像素数
    :return: 填充后的图像，numpy数组，shape为(height+2*pad, width+2*pad)
    """
    height, width = image.shape
    padded_image = np.zeros((height + 2 * pad, width + 2 * pad))
    padded_image[pad : height + pad, pad : width + pad] = image
    return padded_image


def juanji(img, kernel, x0, y0, kernel_size):
    value = 0
    for k in range(kernel_size):
        for t in range(kernel_size):
            ki = k - kernel_size // 2
            kj = t - kernel_size // 2
            img_value = int(img[ki + x0, kj + y0])
            value += img_value * kernel[k, t]
    return value


if __name__ == "__main__":
    guangtiao = cv.imread(
        "./src_fig/detection/center_light_bar.bmp", cv.IMREAD_GRAYSCALE
    )
    h, w = guangtiao.shape  # -0.5-59.5
    plt.imshow(guangtiao, cmap="gray")
    # plt.show()
    # gray=[]
    # #ax = plt.subplot(projection='3d')
    # for i in range(h):
    #     for j in range(w):
    #         #ax.bar3d(i,j,0,0.3,0.3,guangtiao[i,j],color='r')
    #         value = guangtiao[i,j]
    #         gray.append(value)
    # plt.hist(x=gray,bins=60)
    # plt.show()

    # stegger法
    kernel_size = 9
    sigma = kernel_size / 6
    C1 = np.zeros((h, w))
    C2 = np.zeros((h, w))
    kernelx = gauss_gx((kernel_size, kernel_size), sigma)  # 生成核
    kernely = gauss_gy((kernel_size, kernel_size), sigma)
    kernelxx = gauss_gxx((kernel_size, kernel_size), sigma)
    kernelyy = gauss_gyy((kernel_size, kernel_size), sigma)
    kernelxy = gauss_gxy((kernel_size, kernel_size), sigma)
    gx = ss.convolve2d(guangtiao, kernelx, mode="same")
    gy = ss.convolve2d(guangtiao, kernely, mode="same")
    gxx = ss.convolve2d(guangtiao, kernelxx, mode="same")
    gxy = ss.convolve2d(guangtiao, kernelxy, mode="same")
    gyy = ss.convolve2d(guangtiao, kernelyy, mode="same")
    C1 = np.abs(gxx * gyy - gxy**2)
    C2 = np.abs(gxx + gyy)
    eval = np.zeros((h, w))
    nxvec = np.zeros((h, w))
    nyvec = np.zeros((h, w))
    for i in range(h):
        for j in range(w):
            gxx_val = gxx[i, j]
            gxy_val = gxy[i, j]
            gyy_val = gyy[i, j]
            H = np.array([[gxx_val, gxy_val], [gxy_val, gyy_val]])
            eigenvalue, featurevector = np.linalg.eig(H)
            eigenvalue = np.abs(eigenvalue)
            pos = np.argwhere(eigenvalue == np.max(eigenvalue))
            eval[i, j] = eigenvalue[pos]
            nxvec[i, j] = featurevector[0][pos]  #  需要按列读取对应的特征向量
            nyvec[i, j] = featurevector[1][pos]
    midx = []
    midy = []
    th = 8
    for i in range(h):
        for j in range(w):
            gx_val = gx[i, j]
            gy_val = gy[i, j]
            gxx_val = gxx[i, j]
            gxy_val = gxy[i, j]
            gyy_val = gyy[i, j]
            nx_val = nxvec[i, j]
            ny_val = nyvec[i, j]
            t = -(nx_val * gx_val + ny_val * gy_val) / (
                nx_val**2 * gxx_val
                + 2 * nx_val * ny_val * gxy_val
                + ny_val**2 * gyy_val
            )
            tnx = t * nx_val
            tny = t * ny_val
            if np.abs(tnx) <= 0.5 and np.abs(tny) <= 0.5 and eval[i, j] > th:
                midx.append(tnx + i)
                midy.append(tny + j)
    plt.plot(midy, midx, "r*")
    # plt.plot(500,100,'r*')
    plt.show()
